//------------------------------------------------------------------------------
//  <copyright file="FeedbackMessageManaged.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------
#include "Stdafx.h"
#include "KukaLBR4Definitions.h"
#include "FeedbackMessageManaged.h"


using namespace Microsoft::Robotics::Hardware::KukaLBR;

FeedbackMessageManaged::FeedbackMessageManaged()
{
    this->Initialize();
}

FeedbackMessageManaged::FeedbackMessageManaged(KukaFeedbackMessage *pMessage)
{
    this->Initialize();
    this->CopyFrom(pMessage);
}

void FeedbackMessageManaged::CopyFrom(KukaFeedbackMessage *pMessage)
{
    this->ControllerStrategyCommandRegister = pMessage->ControllerStrategyCommandRegister;
    this->Scalar1Register = pMessage->Scalar1Register;
    this->Scalar2Register = pMessage->Scalar2Register;
    this->Scalar3Register = pMessage->Scalar3Register;
    this->Scalar4Register = pMessage->Scalar4Register;
    this->Scalar5Register = pMessage->Scalar5Register;
    this->Scalar6Register = pMessage->Scalar6Register;
    this->Scalar7Register = pMessage->Scalar7Register;
    this->Scalar8Register = pMessage->Scalar8Register;
    this->TimestampInMsRegister = pMessage->TimestampInMsRegister;
    this->FriStateRegister = pMessage->FriStateRegister;
    this->UdpQualityRegister = pMessage->UdpQualityRegister;
    this->CycleTimeInMsRegister = pMessage->CycleTimeInMsRegister;
    this->SafetyLimitInPercentRegister = pMessage->SafetyLimitInPercentRegister;
    this->DrivePowerBitFieldRegister = pMessage->DrivePowerBitFieldRegister;
    this->ControllerStrategyRegister = pMessage->ControllerStrategyRegister;
	this->CurrentControllerStateNumber = pMessage->CurrentControllerStateNumber;
    
    this->CopyFromUnmanagedArray<double>(pMessage->JointPositionCommandRegister, sizeof(pMessage->JointPositionCommandRegister), this->JointPositionCommandRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->CartesianPositionCommandRegister, sizeof(pMessage->CartesianPositionCommandRegister), this->CartesianPositionCommandRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->JointTorqueCommandRegister, sizeof(pMessage->JointTorqueCommandRegister), this->JointTorqueCommandRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->TCPForceTorqueCommandRegister, sizeof(pMessage->TCPForceTorqueCommandRegister), this->TCPForceTorqueCommandRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->JointStiffnessCommandRegister, sizeof(pMessage->JointStiffnessCommandRegister), this->JointStiffnessCommandRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->JointDampingCommandRegister, sizeof(pMessage->JointDampingCommandRegister), this->JointDampingCommandRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->CartesianStiffnessCommandRegister, sizeof(pMessage->CartesianStiffnessCommandRegister), this->CartesianStiffnessCommandRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->CartesianDampingCommandRegister, sizeof(pMessage->CartesianDampingCommandRegister), this->CartesianDampingCommandRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->CartVec1Register, sizeof(pMessage->CartVec1Register), this->CartVec1Register);
    this->CopyFromUnmanagedArray<double>(pMessage->CartVec2Register, sizeof(pMessage->CartVec2Register), this->CartVec2Register);
    this->CopyFromUnmanagedArray<double>(pMessage->CartVec3Register, sizeof(pMessage->CartVec3Register), this->CartVec3Register);
    this->CopyFromUnmanagedArray<double>(pMessage->JointVec1Register, sizeof(pMessage->JointVec1Register), this->JointVec1Register);
    this->CopyFromUnmanagedArray<double>(pMessage->JointVec2Register, sizeof(pMessage->JointVec2Register), this->JointVec2Register);
    this->CopyFromUnmanagedArray<double>(pMessage->JointVec3Register, sizeof(pMessage->JointVec3Register), this->JointVec3Register);
	this->CopyFromUnmanagedArray<double>(pMessage->Frame1Register, sizeof(pMessage->Frame1Register), this->Frame1Register);
	this->CopyFromUnmanagedArray<double>(pMessage->Frame2Register, sizeof(pMessage->Frame2Register), this->Frame2Register);
	this->CopyFromUnmanagedArray<double>(pMessage->Frame3Register, sizeof(pMessage->Frame3Register), this->Frame3Register);
    this->CopyFromUnmanagedArray<double>(pMessage->JointTemperatureRegister, sizeof(pMessage->JointTemperatureRegister), this->JointTemperatureRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->MeasuredJointPositionRegister, sizeof(pMessage->MeasuredJointPositionRegister), this->MeasuredJointPositionRegister);
	this->CopyFromUnmanagedArray<double>(pMessage->MeasuredJointVelocityRegister, sizeof(pMessage->MeasuredJointVelocityRegister), this->MeasuredJointVelocityRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->ReportedCommandedJointPositionRegister, sizeof(pMessage->ReportedCommandedJointPositionRegister), this->ReportedCommandedJointPositionRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->ReportedCommandedJointOffsetRegister, sizeof(pMessage->ReportedCommandedJointOffsetRegister), this->ReportedCommandedJointOffsetRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->MeasuredCartesianPositionRegister, sizeof(pMessage->MeasuredCartesianPositionRegister), this->MeasuredCartesianPositionRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->ReportedCommandedCartesianPositionRegister, sizeof(pMessage->ReportedCommandedCartesianPositionRegister), this->ReportedCommandedCartesianPositionRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->ReportedCommandedCartesianOffsetRegister, sizeof(pMessage->ReportedCommandedCartesianOffsetRegister), this->ReportedCommandedCartesianOffsetRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->MeasuredJointTorqueRegister, sizeof(pMessage->MeasuredJointTorqueRegister), this->MeasuredJointTorqueRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->MeasuredExternalJointTorqueRegister, sizeof(pMessage->MeasuredExternalJointTorqueRegister), this->MeasuredExternalJointTorqueRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->EstimatedTCPForceTorqueRegister, sizeof(pMessage->EstimatedTCPForceTorqueRegister), this->EstimatedTCPForceTorqueRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->JacobianRegister, sizeof(pMessage->JacobianRegister), this->JacobianRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->MassMatrixRegister, sizeof(pMessage->MassMatrixRegister), this->MassMatrixRegister);
    this->CopyFromUnmanagedArray<double>(pMessage->GravityCompensationRegister, sizeof(pMessage->GravityCompensationRegister), this->GravityCompensationRegister);
	this->CopyFromUnmanagedArray<double>(pMessage->LastIssuedJointPositionCommandRegister, sizeof(pMessage->LastIssuedJointPositionCommandRegister), this->LastIssuedJointPositionCommandRegister);

}

template<class T>
bool FeedbackMessageManaged::CopyFromUnmanagedArray(T* from, size_t byteCount, array<T>^ to)
{
    int  minLength = Math::Min(to->Length, (int)(byteCount/sizeof(*from)));

    for (int i = 0 ; i < minLength ; ++i)
    {
        to[i] = from[i];
    }

    return true;
}

void FeedbackMessageManaged::Initialize()
{
    this->ControllerStrategyCommandRegister = 0.0;
    this->Scalar1Register = 0.0;
    this->Scalar2Register = 0.0;
    this->Scalar3Register = 0.0;
    this->Scalar4Register = 0.0;
    this->Scalar5Register = 0.0;
    this->Scalar6Register = 0.0;
    this->Scalar7Register = 0.0;
    this->Scalar8Register = 0.0;
    this->TimestampInMsRegister = 0.0;
    this->FriStateRegister = 0.0;
    this->UdpQualityRegister = 0.0;
    this->CycleTimeInMsRegister = 0.0;
    this->SafetyLimitInPercentRegister = 0.0;
    this->DrivePowerBitFieldRegister = 0.0;
    this->ControllerStrategyRegister = 0.0;
	this->CurrentControllerStateNumber = 0.0;

    this->JointPositionCommandRegister = gcnew array<double>(NUM_JOINTS);
    this->CartesianPositionCommandRegister = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
    this->JointTorqueCommandRegister = gcnew array<double>(NUM_JOINTS);
    this->TCPForceTorqueCommandRegister = gcnew array<double>(CARTESIAN_DIM);
    this->JointStiffnessCommandRegister = gcnew array<double>(NUM_JOINTS);
    this->JointDampingCommandRegister = gcnew array<double>(NUM_JOINTS);
    this->CartesianStiffnessCommandRegister = gcnew array<double>(CARTESIAN_DIM);
    this->CartesianDampingCommandRegister = gcnew array<double>(CARTESIAN_DIM);
    this->CartVec1Register = gcnew array<double>(CARTESIAN_DIM);
    this->CartVec2Register = gcnew array<double>(CARTESIAN_DIM);
    this->CartVec3Register = gcnew array<double>(CARTESIAN_DIM);
    this->JointVec1Register = gcnew array<double>(NUM_JOINTS);
    this->JointVec2Register = gcnew array<double>(NUM_JOINTS);
    this->JointVec3Register = gcnew array<double>(NUM_JOINTS);
	this->Frame1Register = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
	this->Frame2Register = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
	this->Frame3Register = gcnew array<double>(CARTESIAN_FRAME_LENGTH);

    this->JointTemperatureRegister = gcnew array<double>(NUM_JOINTS);
    this->MeasuredJointPositionRegister = gcnew array<double>(NUM_JOINTS);
	this->MeasuredJointVelocityRegister = gcnew array<double>(NUM_JOINTS);
    this->ReportedCommandedJointPositionRegister = gcnew array<double>(NUM_JOINTS);
    this->ReportedCommandedJointOffsetRegister = gcnew array<double>(NUM_JOINTS);
    this->MeasuredCartesianPositionRegister = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
    this->ReportedCommandedCartesianPositionRegister = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
    this->ReportedCommandedCartesianOffsetRegister = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
    this->MeasuredJointTorqueRegister = gcnew array<double>(NUM_JOINTS);
    this->MeasuredExternalJointTorqueRegister = gcnew array<double>(NUM_JOINTS);
    this->EstimatedTCPForceTorqueRegister = gcnew array<double>(CARTESIAN_DIM);
    this->JacobianRegister = gcnew array<double>(CARTESIAN_DIM * NUM_JOINTS);
    this->MassMatrixRegister  = gcnew array<double>(CARTESIAN_DIM * NUM_JOINTS);
    this->GravityCompensationRegister = gcnew array<double>(NUM_JOINTS);
	this->LastIssuedJointPositionCommandRegister = gcnew array<double>(NUM_JOINTS);
}
